#include "offboard.hpp"



void writeCoordinateToFile_det(const Coordinate_txt& coord) {  
    std::ofstream outfile("/home/orangepi/target_coord.txt", std::ios::app); 
    if (outfile.is_open()) {  
        outfile << coord.x << " " << coord.y << " " << coord.z << std::endl; // 写入数据并换行  
        outfile.close();  
    } else {  
        std::cerr << "Unable to open file for writing:/target_coord.txt" << std::endl;  
    }  
}
void writeCoordinateToFile_ekf(const Coordinate_txt& coord) {  
    std::ofstream outfile("/home/orangepi/target_ekf_coord.txt", std::ios::app); 
    if (outfile.is_open()) {  
        outfile << coord.x << " " << coord.y << " " << coord.z << std::endl; // 写入数据并换行  
        outfile.close();  
    } else {  
        std::cerr << "Unable to open file for writing:/target_ekf_coord.txt" << std::endl;  
    }  
}
void writeCoordinateToFile_predict(const Coordinate_txt& coord) {  
    std::ofstream outfile("/home/orangepi/target_predict_coord.txt", std::ios::app); 
    if (outfile.is_open()) {  
        outfile << coord.x << " " << coord.y << " " << coord.z << std::endl; // 写入数据并换行  
        outfile.close();  
    } else {  
        std::cerr << "Unable to open file for writing:/target_predict_coord.txt" << std::endl;  
    }  
}


int main(int argc, char **argv)
{
    ros::init(argc, argv, "off_board_test1");
    ros::NodeHandle nh;
    unionsys_core->detect_serial_number();

    //read parameters from launch file
    nh.param<bool>("use_sitl",use_sitl,false);
    nh.param<double>("max_vel_beishu",max_vel_beishu,1.0);
    nh.param<double>("zhuci_duration",zhuci_duration,0.1);
    
    //1080p (1920 x 1080) @ 30 fps
    //720p (1280 x 720) @ 30 fps
    nh.param<int>("image_width",image_width,640);
    nh.param<int>("image_height",image_height,480);

    nh.param<double>("P_pos",P_pos,1.0);
    nh.param<double>("I_pos",I_pos,0.0);
    nh.param<double>("D_pos",D_pos,0.0);
    nh.param<double>("I_zone",I_zone,1.0);

    //imu_img_delay delta between imu time and img estimate time,
    nh.param<double>("imu_img_delay",imu_img_delay,0);
    nh.param<double>("track_dist",track_dist,5.0);
    nh.param<bool>("attack_air",attack_air,true);

    nh.param<double>("FOCUS",FOCUS,1000);
    nh.param<double>("REALSIZE",REALSIZE,0.4);
    
    
    
    double rotaion_angle = 90*deg2rad;
    R_rotaion<<1,0,0,
        0,cos(rotaion_angle),-sin(rotaion_angle),
        0,sin(rotaion_angle),cos(rotaion_angle);
    double pitch_angle = -0*deg2rad;
    R_up<<cos(pitch_angle),0,sin(pitch_angle),
            0,1,0,
            -sin(pitch_angle),0,cos(pitch_angle);
    

    //Subscriber
    altitude_sub = nh.subscribe("/mavros0/altitude",10,altitude_callback);
    velocitysub = nh.subscribe("/mavros0/local_position/velocity_local",10,velocity_callback);
    local_odom_sub = nh.subscribe("/mavros0/local_position/odom",10,local_odom_callback);
    
    rcin_sub = nh.subscribe("/mavros0/rc/in",10,rcin_callback);//遥控器
    state_sub = nh.subscribe("/mavros0/state", 10, state_cb);

    horizonal_target_sub = nh.subscribe("/intercept/target_horizonal", 10, horizonal_target_cb);

    gps_self_sub = nh.subscribe("/mavros0/global_position/global", 10, gps_self_cb);
    gps_target_sub = nh.subscribe("/mavros1/global_position/global", 10, gps_target_cb);
     
    //Publisher
    local_vel_pub = nh.advertise<geometry_msgs::TwistStamped>("/mavros0/setpoint_velocity/cmd_vel", 10);
    marker_pub = nh.advertise<visualization_msgs::Marker>("/target_box", 1);

    //Service
    arming_client = nh.serviceClient<mavros_msgs::CommandBool>("/mavros0/cmd/arming");
    set_mode_client = nh.serviceClient<mavros_msgs::SetMode>("/mavros0/set_mode");//设置offboard
    set_vechile_ned = nh.serviceClient<mavros_msgs::SetMavFrame>("/mavros0/setpoint_velocity/mav_frame");//设置frame

    //Timer
    px4_control_timer = nh.createTimer(ros::Duration(0.05), px4_control_callback);

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(200);
    // wait for FCU connection
    while(ros::ok() && !current_state.connected){
        std::cout<<"------------>wait for fcu connected..."<<std::endl;
        ros::spinOnce();
        rate.sleep();
    }
    
    offb_set_mode.request.custom_mode = "OFFBOARD";
    arm_cmd.request.value = true;
    setmavframe.request.mav_frame = 8;// set frame
    last_offboard_request = ros::Time::now();
    // if(set_vechile_ned.call(setmavframe) && setmavframe.response.success){
    //     std::cout<<"-->setmavframe success ..."<<std::endl;}
    // else{
    //     std::cout<<"-->setmavframe failed"<<std::endl;    
    // }


    //main loop here
    while(ros::ok()){
        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

void altitude_callback(const mavros_msgs::Altitude &msg)
{
    current_point_z = msg.relative;
}

void local_odom_callback(const nav_msgs::Odometry::Ptr & current_msg)
{
    current_point.x= current_msg->pose.pose.position.x;
    current_point.y= current_msg->pose.pose.position.y;
    current_point.z= current_msg->pose.pose.position.z;
    //current_point.z= current_point_z;
    current_angle.x= current_msg->pose.pose.orientation.x;
    current_angle.y= current_msg->pose.pose.orientation.y;
    current_angle.z= current_msg->pose.pose.orientation.z;
    current_angle.w= current_msg->pose.pose.orientation.w;
    //curr_plane_angle roll pitch yaw (x,y,z)
    curr_plane_angle = unionsys_core->toEulerAngle(current_angle);
    current_angle_Q = Eigen::Quaterniond(current_msg->pose.pose.orientation.w, current_msg->pose.pose.orientation.x,
                                        current_msg->pose.pose.orientation.y, current_msg->pose.pose.orientation.z);
    // R drone

    local_odom_flag = true;
    
    //save imu 
    nav_msgs::Odometry::Ptr msg(new nav_msgs::Odometry(*current_msg));

    msg->header.stamp = ros::Time().fromSec(ros::Time::now().toSec());
    imu_buffer.push_back(msg);

}
void gps_self_cb(const sensor_msgs::NavSatFix &msg)
{
    self_gps_pos.latitude = msg.latitude;
    self_gps_pos.longitude = msg.longitude;
    self_gps_pos.altitude = msg.altitude;
}
void gps_target_cb(const sensor_msgs::NavSatFix &msg)
{
    target_gps_pos.latitude = msg.latitude;
    target_gps_pos.longitude = msg.longitude;
    target_gps_pos.altitude = msg.altitude;
    target_local_coord(0) = current_point.x + (target_gps_pos.longitude - self_gps_pos.longitude)*111318.0*cos((target_gps_pos.latitude + self_gps_pos.latitude)/2*deg2rad);
    target_local_coord(1) = current_point.y + (target_gps_pos.latitude - self_gps_pos.latitude)*110946.0;
    target_local_coord(2) = current_point.z + (target_gps_pos.altitude - self_gps_pos.altitude);
    target_yaw_angle = atan2((target_local_coord(1)-current_point.y),(target_local_coord(0)-current_point.x));

    target_gps_receive = ros::Time::now().toSec();
}


void vel_command_send(geometry_msgs::Point vel, double yaw_rate)
{
    vel_command_cnt++;
    geometry_msgs::Vector3 linear_temp;
    geometry_msgs::Vector3 angular_temp;

    geometry_msgs::TwistStamped msg_vel_pub;
    linear_temp.x = vel.x;//forward +
    linear_temp.y = vel.y;//left + roll_vel
    linear_temp.z = vel.z;//up +
    
    angular_temp.x = 0;
    angular_temp.y = 0; 
    angular_temp.z = yaw_rate;
    msg_vel_pub.header.stamp = ros::Time::now();
    msg_vel_pub.header.seq=vel_command_cnt;
    msg_vel_pub.twist.linear=linear_temp;
    msg_vel_pub.twist.angular=angular_temp;

    local_vel_pub.publish(msg_vel_pub);
}


void changeFSMExecState(FSM_STATE new_state, std::string pos_call) 
{
    int    pre_s        = int(exec_state);
    exec_state         = new_state;
    std::cout << "[" + pos_call + "]: from " + state_str[pre_s] + " to " + state_str[int(new_state)] << std::endl;
}


void printFSMExecState() {
    std::cout << "state: " + state_str[int(exec_state)] << std::endl;
}

//velocity_limit限制不要超过3
//bias transform to unit:meter
void position_pid_control(Eigen::Vector3d target_position, float velocity_limit, float set_yaw_angle)
{
    float vx = 0.0, vy = 0.0, vxp = 0.0, vyp = 0.0, vxi = 0.0, vyi = 0.0, vxd = 0.0, vyd = 0.0, vx_lim=0.0, vy_lim=0.0;
    float yaw_rate = 0.0, yaw_rate_p = 0.0, vz = 0;

    geometry_msgs::Point error_pos;
    float dt = ros::Time::now().toSec()-last_calc_time;
    if(dt>1.0) //dt too long maybe need init dt
    {
        last_calc_time = ros::Time::now().toSec();
        return;
    }
    //calculate velocity
    vx = P_pos*(target_position(0)-current_point.x)-D_pos * current_vel.x+feedforward_x;
    vy = P_pos*(target_position(1)-current_point.y)-D_pos * current_vel.y+feedforward_y;

    vx_lim = unionsys_core->limit_velocity(vx, vy, velocity_limit).x;
    vy_lim = unionsys_core->limit_velocity(vx, vy, velocity_limit).y;

    //yaw control
    float error_yaw = set_yaw_angle*rad2deg; //angle
    //Drone turns at the smallest angle
    yaw_rate_p = P_yaw * error_yaw;
    yaw_rate = yaw_rate_p ;
    if (abs(error_yaw) >= YAW_RATE_LIMIT) {
        yaw_rate = limit(yaw_rate, YAW_RATE_LIMIT);
    } 

    //height control 
    if(attack_air)
    {
        vz = 1.5*P_z * (target_position(2)-current_point.z)-D_pos * current_vel.z; 
    }
    else
    {
        //when attack ground target ,need swoop
        //--------------
        //              \ 
        //               \ 
        //                \ 
        //                 target
    }
    
    float z_lim = 3.0; 
    vz = limit(vz, z_lim);
    velocity_expected.x = vx_lim;
    velocity_expected.y = vy_lim;
    velocity_expected.z = vz;
    
    attitude_expect.z = yaw_rate*deg2rad;
    last_calc_time = ros::Time::now().toSec();
}

void set_cruise_state()
{
    velocity_expected.x = 0.0;
    velocity_expected.y = 0.0;
    velocity_expected.z = 0.0;
    vel_command_send(velocity_expected, 0);
  
}

void draw_box(Box temp_box, int id, double width, double red, double green, double blue)
{
    //draw box on rviz
    visualization_msgs::Marker line_strip;
    line_strip.header.frame_id = "map";
    line_strip.header.stamp = ros::Time::now();
    line_strip.ns = "target_box";//namespace
    line_strip.action = visualization_msgs::Marker::ADD;
    line_strip.pose.orientation.w = 1.0;
    line_strip.id = id; //unique id, useful when multiple markers exist.
    line_strip.type = visualization_msgs::Marker::LINE_STRIP; //marker type
    line_strip.lifetime = ros::Duration(0.5);
    line_strip.scale.x = width; //width of the line
    line_strip.color.r = red; 
    line_strip.color.g = green; 
    line_strip.color.b = blue;
    line_strip.color.a = 1.0; //alpha ,set 1
    geometry_msgs:: Point p[8];
    p[0].x = temp_box.x_min;     p[0].y = temp_box.y_max;     p[0].z = temp_box.z_max;
    p[1].x = temp_box.x_min;     p[1].y = temp_box.y_min;     p[1].z = temp_box.z_max;
    p[2].x = temp_box.x_min;     p[2].y = temp_box.y_min;     p[2].z = temp_box.z_min;
    p[3].x = temp_box.x_min;     p[3].y = temp_box.y_max;     p[3].z = temp_box.z_min;
    p[4].x = temp_box.x_max;     p[4].y = temp_box.y_max;     p[4].z = temp_box.z_min;
    p[5].x = temp_box.x_max;     p[5].y = temp_box.y_min;     p[5].z = temp_box.z_min;
    p[6].x = temp_box.x_max;     p[6].y = temp_box.y_min;     p[6].z = temp_box.z_max;
    p[7].x = temp_box.x_max;     p[7].y = temp_box.y_max;     p[7].z = temp_box.z_max;
    //LINE_STRIP link 0-1 1-2 2-3 only
    for (int i = 0; i < 8; i++){
        line_strip.points.push_back(p[i]);
    }
    //so we need to add every links mannualy
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[3]);
    line_strip.points.push_back(p[2]);
    line_strip.points.push_back(p[5]);
    line_strip.points.push_back(p[6]);
    line_strip.points.push_back(p[1]);
    line_strip.points.push_back(p[0]);
    line_strip.points.push_back(p[7]);
    line_strip.points.push_back(p[4]);
    marker_pub.publish(line_strip);
    
    //end draw    
}
void horizonal_target_cb(const swarm_msgs::BoundingBoxes &msg)
{
    //视觉伺服
    dt_vision = ros::Time::now().toSec()-last_vision_time;
    if(dt_vision>1.0) //dt too long maybe need init dt
    {
        last_vision_time = ros::Time::now().toSec();
        return;
    }
    last_vision_time = ros::Time::now().toSec();
    //计算框大小
    if(msg.bounding_boxes.size()>0)
    {
        double box_time = ros::Time::now().toSec();
        for(int i=0;i<msg.bounding_boxes.size();i++)
        {
//            std::cout<<"=======>>centerpoint: "<<(msg.bounding_boxes[i].xmax+msg.bounding_boxes[i].xmin)/2.0<<","<<(msg.bounding_boxes[i].ymax+msg.bounding_boxes[i].ymin)/2.0<<std::endl;
            box_h = msg.bounding_boxes[i].xmax-msg.bounding_boxes[i].xmin;
            box_v = msg.bounding_boxes[i].ymax-msg.bounding_boxes[i].ymin;
            double box_size = sqrt(box_h * box_h + box_v * box_v);
            // if(box_size<20) //筛选太小的框
            // {
            //     continue;
            // }

            Bias_H = image_width/2 -(msg.bounding_boxes[i].xmax+msg.bounding_boxes[i].xmin)/2.0 ; 
            Bias_V = image_height/2 - (msg.bounding_boxes[i].ymax+msg.bounding_boxes[i].ymin)/2.0;
            Bias_V = Bias_V * 552 / 480;
            
            double alpha = atan2(Bias_V,FOCUS);
            double beta = atan2(Bias_H,FOCUS);
            double distance = REALSIZE * sqrt(Bias_H*Bias_H + Bias_V*Bias_V + FOCUS*FOCUS) /  box_size;
            std::cout<<"--------------------------------distance->"<<distance<<std::endl;
            error_yaw_esitmate = alpha;
            std::cout<<"========>>self angle: "<<beta*180/3.14<<","<<alpha*180/3.14<<std::endl;
            P_Cam(0) = distance * cos(alpha) * cos(beta);
            P_Cam(1) = distance * cos(alpha) * sin(beta);
            P_Cam(2) = distance * sin(alpha);
            //std::cout<<"====>imu_img_delay: "<<imu_img_delay<<std::endl; 
            while (!imu_buffer.empty())
            {
                double imu_time = imu_buffer.front()->header.stamp.toSec()+imu_img_delay;
                if((box_time-imu_time)<0.1)
                {
                    geometry_msgs::Quaternion current_angle_old;
                    current_angle_old.x= imu_buffer.front()->pose.pose.orientation.x;
                    current_angle_old.y= imu_buffer.front()->pose.pose.orientation.y;
                    current_angle_old.z= imu_buffer.front()->pose.pose.orientation.z;
                    current_angle_old.w= imu_buffer.front()->pose.pose.orientation.w;
                    Eigen::Quaterniond current_angle_Q_old = Eigen::Quaterniond(current_angle_old.w, current_angle_old.x,
                                                        current_angle_old.y, current_angle_old.z);
                    // R drone
                    Eigen::Matrix3d rotation_vehicle_old = current_angle_Q_old.toRotationMatrix();                   
                    P_Cam = R_rotaion * P_Cam;
                    P_Cam = R_up * P_Cam;
                    P_Cam = rotation_vehicle_old * P_Cam;
                    P_Cam(0) += imu_buffer.front()->pose.pose.position.x;
                    P_Cam(1) +=imu_buffer.front()->pose.pose.position.y;
                    P_Cam(2) +=imu_buffer.front()->pose.pose.position.z;
                    //draw current target box
                    Box vision_box;
                    vision_box.x_min = P_Cam(0)-REALSIZE/2.0;
                    vision_box.y_min = P_Cam(1)-REALSIZE/2.0;
                    vision_box.z_min = P_Cam(2)-REALSIZE/2.0;
                    vision_box.x_max = P_Cam(0)+REALSIZE/2.0;
                    vision_box.y_max = P_Cam(1)+REALSIZE/2.0;
                    vision_box.z_max = P_Cam(2)+REALSIZE/2.0;
                    vision_box.id = 0;
                    draw_box(vision_box, i, 0.2, 1, 0, 0);
                    // std::cout<<"-target_point-> "<<P_Cam(0)<<","<<P_Cam(1)<<","<<P_Cam(2)<<std::endl;
                    if(vel_xyz>1.0)
                    {
                        if(distance>10)
                        {
                            ekf_predict_time = 0.5;
                        }
                        else
                        {
                            ekf_predict_time =0.1 + distance/vel_xyz;
                        }
                        
                    }
                    else
                    {
                        ekf_predict_time = 0.5;
                    }
                    ekf_predict_process();
                    imu_buffer.pop_front();
                    break;
                } 
                imu_buffer.pop_front();
            }
        }
        have_target_time = ros::Time::now().toSec();
    }
}

void ekf_predict_process()
{
    // Record Target Position
    Coordinate_txt coord2 = {P_Cam(0), P_Cam(1), P_Cam(2)}; // 示例坐标  
        writeCoordinateToFile_det(coord2);

    // EKF2
    z_measured << P_Cam(0),P_Cam(1),P_Cam(2); // 赋值  
    ekf.set_dt(dt_vision);
    ekf.predict();
    ekf.update(z_measured); 
    Eigen::VectorXd x_ = ekf.getState();
    P_Cam_Ekf(0) = x_[0];
    P_Cam_Ekf(1) = x_[1];
    P_Cam_Ekf(2) = x_[2];
    // Record EKF2 Result
    coord2 = {P_Cam_Ekf(0), P_Cam_Ekf(1), P_Cam_Ekf(2)}; // 示例坐标 
        writeCoordinateToFile_ekf(coord2);
    feedforward_x = x_[3];
    feedforward_y = x_[4];
    // Predict with EKF,  t = dist_to_target/self_vel
    //std::cout<<"--->ekf_predict_time: "<<ekf_predict_time<<std::endl;
    P_Cam_Latency_Eliminate(0) = P_Cam_Ekf(0)+x_[3]*ekf_predict_time+0.5*x_[6]*ekf_predict_time*ekf_predict_time;
    P_Cam_Latency_Eliminate(1) = P_Cam_Ekf(1)+x_[4]*ekf_predict_time+0.5*x_[7]*ekf_predict_time*ekf_predict_time;
    P_Cam_Latency_Eliminate(2) = P_Cam_Ekf(2);//+x_[5]*delay_temp+0.5*x_[8]*delay_temp*delay_temp;
    // Record predicted Result
    coord2 = {P_Cam_Latency_Eliminate(0), P_Cam_Latency_Eliminate(1), P_Cam_Latency_Eliminate(2)}; // 示例坐标 
    writeCoordinateToFile_predict(coord2);
    // draw result of ekf and predict
    // ekf
    Box vision_box3;
    vision_box3.x_min = P_Cam_Latency_Eliminate(0)-REALSIZE/2.0;
    vision_box3.y_min = P_Cam_Latency_Eliminate(1)-REALSIZE/2.0;
    vision_box3.z_min = P_Cam_Latency_Eliminate(2)-REALSIZE/2.0;
    vision_box3.x_max = P_Cam_Latency_Eliminate(0)+REALSIZE/2.0;
    vision_box3.y_max = P_Cam_Latency_Eliminate(1)+REALSIZE/2.0;
    vision_box3.z_max = P_Cam_Latency_Eliminate(2)+REALSIZE/2.0;
    vision_box3.id = 201;
    draw_box(vision_box3, 201, 0.2, 0.0, 0.0, 1.0);
    // desire where to track
    Eigen::Vector3d target_vector;
    target_vector(0) = P_Cam_Latency_Eliminate(0)-current_point.x;
    target_vector(1) = P_Cam_Latency_Eliminate(1)-current_point.y;
    target_vector(2) = P_Cam_Latency_Eliminate(2)-current_point.z;
    double tar_angle = atan2(target_vector(1),target_vector(0));
    P_Cam_Track(0) = P_Cam_Latency_Eliminate(0)-track_dist*cos(tar_angle);
    P_Cam_Track(1) = P_Cam_Latency_Eliminate(1)-track_dist*sin(tar_angle);
    P_Cam_Track(2) = P_Cam_Latency_Eliminate(2)+0.2;
    Box vision_box4;
    vision_box4.x_min = P_Cam_Track(0)-REALSIZE/2.0;
    vision_box4.y_min = P_Cam_Track(1)-REALSIZE/2.0;
    vision_box4.z_min = P_Cam_Track(2)-REALSIZE/2.0;
    vision_box4.x_max = P_Cam_Track(0)+REALSIZE/2.0;
    vision_box4.y_max = P_Cam_Track(1)+REALSIZE/2.0;
    vision_box4.z_max = P_Cam_Track(2)+REALSIZE/2.0;
    vision_box4.id = 302;
    draw_box(vision_box4, 301, 0.2, 0.5, 0.0, 0.5);
    // std::cout<<"--track_point-> "<<P_Cam_Track(0)<<","<<P_Cam_Track(1)<<","<<P_Cam_Track(2)<<std::endl;
}
void rcin_callback(const mavros_msgs::RCIn & rcvalue)
{
    channel1_value = rcvalue.channels[0]; //roll
    channel2_value = rcvalue.channels[1]; //pitch
    channel3_value = rcvalue.channels[2]; //thrust
    channel4_value = rcvalue.channels[3]; //yaw
    channel5_value = rcvalue.channels[4]; //model position att stable
    channel6_value = rcvalue.channels[5]; //kill switch
    channel7_value = rcvalue.channels[6]; //A button
    channel8_value = rcvalue.channels[7]; //B button
    channel_test = rcvalue.channels[9];
    //img_delay = (channel_test-1500)/450.0;
}

void velocity_callback(const geometry_msgs::TwistStamped &msg)
{
    current_vel.x = msg.twist.linear.x;
    current_vel.y = msg.twist.linear.y;
    current_vel.z = msg.twist.linear.z;
    vel_xyz = sqrt(current_vel.x*current_vel.x+current_vel.y*current_vel.y);
}

void px4_control_callback(const ros::TimerEvent& /*event*/) 
{
    // printFSMExecState();
    if(print_state_cnt>100)
    {
        //print state per second
        printFSMExecState();
        print_state_cnt = 0;
    }
    print_state_cnt++;
    
    switch (exec_state) { 
    case INIT: {
        if (!local_odom_flag) {
            break;
        }
        if(use_sitl)
        {   //只有仿真会自动进offboard
            // changeFSMExecState(TAKEOFF, "offboard_fsm");
        }
        else
        {
            changeFSMExecState(CRUISE, "offboard_fsm");
        }
    break;
    }
    // case TAKEOFF: {
    //     if( current_state.mode != "OFFBOARD" &&
    //     (ros::Time::now() - last_offboard_request > ros::Duration(1.0))){
    //         if( set_mode_client.call(offb_set_mode) &&
    //             offb_set_mode.response.mode_sent){
    //             ROS_INFO("Offboard enabled");
    //         }
    //         last_offboard_request = ros::Time::now();
    //     } 
    //     else {
    //         if( !current_state.armed &&
    //             (ros::Time::now() - last_offboard_request > ros::Duration(1.0))){
    //             if( arming_client.call(arm_cmd) &&
    //                 arm_cmd.response.success){
    //                 ROS_INFO("Vehicle armed.");
    //             }
    //             last_offboard_request = ros::Time::now();
    //         }
    //     }
    //     velocity_expected.x = 0.0;
    //     velocity_expected.y = 0.0;
    //     velocity_expected.z = 1.0;
    //     vel_command_send(velocity_expected, 0);
    //     if(current_point.z>2.0)
    //     {
    //         changeFSMExecState(CRUISE, "offboard_fsm");
    //     }
    // break;
    // }
    case CRUISE: {
        set_cruise_state();
        if(current_state.mode == "OFFBOARD")
        {
            changeFSMExecState(ATTACK, "offboard_fsm");  
        }
    break;
    }
    case ATTACK: {
        if(ros::Time::now().toSec()-have_target_time>1.0)
        {
            if(ros::Time::now().toSec()-target_gps_receive<0.5)
            {
                double max_vel_temp = std::min(vel_xyz+2,max_vel_beishu);
                position_pid_control(target_local_coord, max_vel_temp,target_yaw_angle);
                vel_command_send(velocity_expected, attitude_expect.z);
            }
            else
            {
                set_cruise_state();
            }

        }
        else
        { 
            double max_vel_temp = std::min(vel_xyz+2,max_vel_beishu);
            position_pid_control(P_Cam_Track, max_vel_temp, error_yaw_esitmate);
            vel_command_send(velocity_expected, attitude_expect.z);
            std::cout<<"--->vel send: "<<velocity_expected.x<<","<<velocity_expected.y<<","<<velocity_expected.z<<std::endl;
        }
    break;
    }

    }
}

